#include <ros/ros.h>
#include <tf2_ros/buffer.h>
#include <tf2_ros/transform_listener.h>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2/LinearMath/Matrix3x3.h>
#include <tf2/utils.h>
#include <geometry_msgs/PoseStamped.h>
#include <nav_msgs/Path.h>
#include "core/common/core.h"
#include "perception/costmap_2d/costmap_2d_ros.h"
#include "path_generate/path_base.h"
#include "path_generate/p2p/astar.h"
#include "path_generate/p2p/jps.h"

class TestPathGen{
    public:
        TestPathGen(tf2_ros::Buffer* tf_buffer);
        ~TestPathGen() = default;

        // 地图回调函数
        void MapCallback(const nav_msgs::OccupancyGrid::Ptr& msg);
        // Rviz位姿回调函数
        void RvizPosesCallback(const geometry_msgs::PoseStampedConstPtr& msg);

        
    private:
        ros::NodeHandle nh_;
        tf2_ros::Buffer* const tfBuffer_;
        std::shared_ptr<PlannerBase> PlannerBasePtr_;
        ros::Subscriber rvizPoseSub_;
        ros::Subscriber mapSub_; 
        ros::Publisher costmapGlobalPub_;

        vector<geometry_msgs::PoseStamped> poses_; 
        boost::shared_ptr<costmap_2d::Costmap2DROS> costmapGlobalPtr_; 
};


TestPathGen::TestPathGen(tf2_ros::Buffer* tf_buffer):tfBuffer_(tf_buffer){
    costmapGlobalPtr_ = boost::make_shared<costmap_2d::Costmap2DROS>("global_costmap", *tfBuffer_);
    PlannerBasePtr_ = std::make_shared<AstarPlanner>();
    // PlannerBasePtr_ = std::make_shared<JPSPlanner>();
    PlannerBasePtr_->setDebugMode(true);
    costmapGlobalPub_ = nh_.advertise<nav_msgs::OccupancyGrid>("/global_costmap", 1);
    rvizPoseSub_ = nh_.subscribe("/move_base_simple/goal", 10, &TestPathGen::RvizPosesCallback, this);
    mapSub_ = nh_.subscribe("/map", 10, &TestPathGen::MapCallback, this);
}

// 地图回调函数
void TestPathGen::MapCallback(const nav_msgs::OccupancyGrid::Ptr& msg){
    LOG(INFO) << "Receive a map. width=" << msg->info.width 
        << ", height=" << msg->info.height
        << ", resolution=" << msg->info.resolution;
    // if(costmapGlobalPtr_)
    //     costmapGlobalPtr_->setMapData(msg);
    // if(PlannerBasePtr_)
    //     PlannerBasePtr_->setMap(msg);
}

// Rviz位姿回调函数
void TestPathGen::RvizPosesCallback(const geometry_msgs::PoseStampedConstPtr& msg){
    LOG(INFO) << "Receive a pose. x=" << msg->pose.position.x
        << ", y=" << msg->pose.position.y
        << ", yaw=" << tf2::getYaw(msg->pose.orientation);
    int poses_size = poses_.size();
    switch(poses_.size()){
    case 0:
        poses_.emplace_back(*msg);
        LOG(INFO) << "set start point. poses_size: " 
            << poses_.size();
        break;
    case 1:
        poses_.emplace_back(*msg);
        LOG(INFO) << "set goal point. poses_size: " 
            << poses_.size();
        // break;
    case 2:
        LOG(INFO) << "planning start... poses_size: " 
            << poses_.size();
        {
            // nav_msgs::OccupancyGrid global_map;
            // costmapGlobalPtr_->updateCostmap();
            // costmapGlobalPtr_->getOccupancyGrid(global_map);
            // costmapGlobalPub_.publish(global_map);
            nav_msgs::OccupancyGrid global_map ;
            costmapGlobalPtr_->getOccupancyGrid(global_map);
            auto global_map_ptr = boost::make_shared<nav_msgs::OccupancyGrid>(global_map);
            PlannerBasePtr_->setMap(global_map_ptr);            
            // 开始规划路径
            vector<geometry_msgs::Pose> poses;
            PlannerBasePtr_->genPath(poses_.front().pose, poses_.back().pose, poses);
        }
        // 清空数据
        poses_.clear();
        LOG(INFO) << "clear. poses_size: " << poses_.size();
        break;
    default:
        // 清空数据
        poses_.clear();
        LOG(INFO) << "clear. poses_size: " << poses_.size();
        break;
    }
}

int main(int argc, char** argv){
    ros::init(argc, argv, "test_path_generate");

    string glog_dir = ros::param::param<string>("/glog_dir", "/home/xxty/user_config/logs");
    common::glog::init(glog_dir, "test_path_generate");

    setlocale(LC_ALL,"");   //ROS_INFO 中文显示, 需要添加头文件#include <sstream>
    // ROS_INFO("test_path_generate start...");
    LOG(INFO) << "test_path_generate start...";

    tf2_ros::Buffer tfBuffer(::ros::Duration(10));
    tf2_ros::TransformListener tfListener(tfBuffer);
    TestPathGen test_path_generate(&tfBuffer);
    ros::spin();

    common::glog::shutdown();

    return 0;
}